#include "leos_thread.h"
#include "leos_conf.h"
#include "leos_hw.h"
#include "leos_service.h"

le_list_t le_thread_priority_table[LE_THREAD_PRIORITY_MAX];

static le_uint32_t  le_thread_ready_priority_group;
static le_int32_t   le_schedule_lock_nest;

void le_system_scheduler_init(void)
{
    le_schedule_lock_nest = 0;

    for (int i = 0; i < LE_THREAD_PRIORITY_MAX; i++) {
        le_list_init(&le_thread_priority_table[i]);
    }

    le_current_thread = LE_NULL;

    le_thread_ready_priority_group = 0;
}

void le_system_scheduler_start(void)
{
    le_thread_t to_thread = LE_NULL;
    le_uint8_t highest_ready_priority = le_find_first_set_bit(le_thread_ready_priority_group) - 1;
                    

    to_thread = le_container_of(le_thread_priority_table[highest_ready_priority].next, 
                                struct le_thread, 
                                tlist);
    
    le_current_thread = to_thread;

    le_hw_context_switch_to((le_uint32_t)&to_thread->sp);
}

/* 将线程添加到就绪链表，更新状态，更新 就绪优先级组 */
void le_schedule_insert_thread(le_thread_t thread)
{
    le_int32_t level = le_hw_interrupt_disable();

    thread->state = LE_THERAD_STATE_READY;

    le_list_insert_before(&thread->tlist, &le_thread_priority_table[thread->current_priority]);

    le_thread_ready_priority_group |= (1L << thread->current_priority);

    le_hw_interrupt_enable(level);
}

/* 从就绪链表中移除该线程，判断当前优先级下是否还存在别的线程，没有的话清除 就绪优先级组 中的对应位 */
void le_schedule_remove_thread(le_thread_t thread)
{
    le_int32_t level = le_hw_interrupt_disable();

    le_list_remove(&thread->tlist);

    if (le_list_isempty(&le_thread_priority_table[thread->current_priority])) {
        le_thread_ready_priority_group &= ~(1L << thread->current_priority);
    }

    le_hw_interrupt_enable(level);
}

/* 根据 就绪优先级组 获取当前最高优先级，从对应链表中取第一个节点，获取线程指针，线程切换 */
void le_schedule(void)
{
    le_thread_t from_thread = LE_NULL;
    le_thread_t to_thread = LE_NULL;

    le_int32_t level = le_hw_interrupt_disable();

    if (le_schedule_lock_nest == 0) {
        le_uint8_t highest_ready_priority = le_find_first_set_bit(le_thread_ready_priority_group) - 1;
                        
        to_thread = le_container_of(le_thread_priority_table[highest_ready_priority].next, 
                                    struct le_thread, 
                                    tlist);

        if (to_thread != le_current_thread) {
            from_thread = le_current_thread;
            le_current_thread = to_thread;

            le_hw_context_switch((le_uint32_t)&from_thread->sp, (le_uint32_t)&to_thread->sp);
        }
    }

    le_hw_interrupt_enable(level);
}

void le_enter_critical(void)
{
    le_int32_t level = le_hw_interrupt_disable();

    le_schedule_lock_nest++;

    le_hw_interrupt_enable(level);
}

void le_exit_critical(void)
{
    le_int32_t level = le_hw_interrupt_disable();

    le_schedule_lock_nest--;

    if (le_schedule_lock_nest > 0) {
        le_hw_interrupt_enable(level);
        return;
    }

    le_schedule_lock_nest = 0;

    le_hw_interrupt_enable(level);

    if (le_current_thread) {
        le_schedule();
    }
}
